
#include <glog/logging.h>

#include "publisher/publisher.h"

#include "pangolin_viewer/viewer.h"

#include "read_data.h"

using namespace cartographer;
using namespace pangolin_viewer;

static bool use_pangolin = true;

std::string data_path = "/media/zl/1E2E59842E5955BD/SLAM/data/2012-01-08/";

int main(int argc, char** argv) {

    std::shared_ptr<Publisher> publisher_ptr = std::make_shared<Publisher>();
    if(use_pangolin) {
        pangolin_viewer::viewer* viewer_ptr = new pangolin_viewer::viewer(publisher_ptr);
        std::thread* viewer_thread_ptr = new thread(&viewer::run, viewer_ptr);      
    }

    double last_left = 0;
    double last_right = 0;
    double last_wheel_time = 0;
    bool used_simple_point = true;

    // Options options_;
    Eigen::Matrix3d Rwo = Eigen::Matrix3d::Identity();
    Eigen::Vector3d pwo = Eigen::Vector3d::Zero();

    std::string data_stamp_path = data_path + "data_stamp.csv";
    std::ifstream fin(data_stamp_path);
    if (!fin) {
        return -1;
    }
    
    std::vector<std::string> line_data_vec;
    line_data_vec.reserve(3);
    std::string line_str, value_str;
    read_data* read_input_data = new read_data(data_path, SensorDataType::NCLT);

    std::unordered_map<std::string, std::string> time_encoder_map = read_input_data->read_only_nclt_data("odometry_mu_100hz.csv");
    std::unordered_map<std::string, std::string> time_imu_map = read_input_data->read_only_nclt_data("ms25.csv");
    std::unordered_map<std::string, std::string> time_gps_map; 
    std::unordered_map<std::string, std::string> time_vrs_gps_map;
    std::unordered_map<std::string, LaserScan> time_sick_middle_map = read_input_data->read_only_nclt_laser_data("hokuyo_30m.bin");
    // std::unordered_map<std::string, std::string> time_velodyne_left_map;

    // for (const auto& pair : time_sick_middle_map) {
    //     const std::string& key = pair.first;    // 获取键
    //     const LaserScan& value = pair.second;   // 获取值

    //     LOG(INFO) << "key : " << key << std::endl;
    //     LOG(INFO) << "value : " << std::to_string(value.scan_time) << std::endl;

    //     sleep(5);

    // }


    // double first_timestamp = read_input_data->get_origin_timestamp();

    // init options
    NodeOptions node_options;
    node_options.trajectory_id = 0;

    TrajectoryOptions trajectory_options;

    Node node(node_options, read_input_data->get_ext(), publisher_ptr);
    node.StartTrajectoryWithDefaultTopics(trajectory_options);
    bool is_first_frame = false;
    // start add data
    while (std::getline(fin, line_str)) {
        line_data_vec.clear();
        std::stringstream ss(line_str);

        // LOG(INFO) << "line_str : " << line_str << std::endl;

        while (std::getline(ss, value_str, ',')) { 
            // LOG(INFO) << "value_str : " << value_str.size() << ", " << value_str << std::endl;
            line_data_vec.push_back(value_str); 
        }
        const double kToSecond = 1e-9;
        if(line_data_vec.empty()) {
            continue;
        }
        const std::string& time_str = line_data_vec[0];
        
        if(!is_first_frame) {
            // first_timestamp = std::stod(time_str) * kToSecond;
            is_first_frame = true;
            continue;
        }

        // const double timestamp = std::stod(time_str) * kToSecond - first_timestamp;
        const double timestamp = std::stod(time_str) * kToSecond;

        const std::string& sensor_type = trim_copy(line_data_vec[1]);

        // LOG(INFO) << "timestamp : " << timestamp << std::endl;

        // std::cout << "sensor_type : " << sensor_type << std::endl;
        // std::cout << "time_imu_map : " << time_imu_map.size() << std::endl;

        if (sensor_type == "Microstrain_IMU") {            
            if (time_imu_map.find(time_str) == time_imu_map.end()) {
                LOG(ERROR) << "[main]: Failed to find imu data at time: " << time_str << std::endl;
                continue;
            }
            const std::string& imu_str = time_imu_map.at(time_str);
            // std::cout << "imu_str ======= : " << imu_str << std::endl;

            if(imu_str.size() < 10) {
                continue;
            }   
            std::stringstream imu_ss(imu_str);
            line_data_vec.clear();
            while (std::getline(imu_ss, value_str, ',')) { line_data_vec.push_back(value_str); }
            if(line_data_vec.empty()) {
                continue;
            }
            double time = timestamp;
            // std::cout << "time : " << time << std::endl;
            double gx = std::stod(line_data_vec[7]);
            double gy = std::stod(line_data_vec[8]);
            double gz = std::stod(line_data_vec[9]);
            double ax = std::stod(line_data_vec[4]);
            double ay = std::stod(line_data_vec[5]);
            double az = std::stod(line_data_vec[6]);  
            // todo : add data
            sensor::ImuData imu_data;
            imu_data.time = common::FromSecond(time);
            imu_data.linear_acceleration = Eigen::Vector3d(ax, ay, az);
            imu_data.angular_velocity = Eigen::Vector3d(gx, gy, gz);

            // std::cout << "imu_data.linear_acceleration : " << imu_data.linear_acceleration.transpose() << std::endl;
            // std::cout << "imu_data.angular_velocity : " << imu_data.angular_velocity.transpose() << std::endl;

            // node.HandleImuMessage(node_options.trajectory_id, sensor_type, imu_data);

        } else if (sensor_type == "Vehicle_Odometry") {
             if (time_encoder_map.find(time_str) == time_encoder_map.end()) {
                LOG(ERROR) << "[main]: Failed to find encoder data at time: " << time_str << std::endl;
                continue;
            }

            const std::string& encoder_str = time_encoder_map.at(time_str);
            std::stringstream enc_ss(encoder_str);
            line_data_vec.clear();
            while (std::getline(enc_ss, value_str, ',')) { line_data_vec.push_back(value_str); }
            if(line_data_vec.empty()) {
                continue;
            }
     
            // std::cout << "line_data_vec ======= : " << line_data_vec.size() << std::endl;

            // todo : add wheel data 
            pwo = Eigen::Vector3d(std::stod(line_data_vec[1]), std::stod(line_data_vec[2]), std::stod(line_data_vec[3]));
            Rwo = eulerToRotationMatrix(0.0, 0.0, std::stod(line_data_vec[6]));
            // Rwo = eulerToRotationMatrix(std::stod(line_data_vec[4]), std::stod(line_data_vec[5]), std::stod(line_data_vec[6]));

            double time = timestamp;

            sensor::OdometryData odometry_data;
            odometry_data.time = common::FromSecond(time);
            odometry_data.pose = transform::Rigid3d(pwo, Eigen::Quaterniond(Rwo));

            node.HandleOdometryMessage(node_options.trajectory_id, sensor_type, odometry_data);

            Eigen::Vector2d p = pwo.head<2>();
            double theta = std::atan2(Rwo(1, 0), Rwo(0, 0));
            publisher_ptr->PublishOdometryPose(Eigen::Vector3d(p[0], p[1], theta));

            // LOG(INFO) << "theta ========= : " << theta << std::endl;
            LOG(INFO) << "p ========= : " << p.transpose() << " theta : " << theta << std::endl;
        }else if (sensor_type == "gps") {
           
        } else if (sensor_type == "vrs") {
          
        } else if (sensor_type == "Hokuyo_30m") {

            // LOG(INFO) << "time_str ========= : " << time_str.size() << std::endl;
            // LOG(INFO) << "time_str ========= : " << time_str << std::endl;

            if (time_sick_middle_map.find(time_str) == time_sick_middle_map.end()) {
                LOG(ERROR) << "[main]: Failed to find Hokuyo_30m data at time: " << time_str << std::endl;
                continue;
            }

            LaserScan scan = time_sick_middle_map.at(time_str);

            // LOG(INFO) << "laser scan time_str ========= : " << time_str << std::endl;

            // add laser scan
            node.HandleLaserScanMessage(node_options.trajectory_id, sensor_type, scan);
            
            // publisher_ptr->PublishOriginPointCloud(scan.x_coords, scan.y_coords);

            //LOG(INFO) << "timestamp : " << timestamp;
        } else if(sensor_type == "velodyne_left") {


        }  else if (sensor_type == "stereo") {
  
        }

        usleep(5000);
    }

    return 0;
}